#include <stdio.h>
#include <stdlib.h>
#include <ctype.h>
#include <string.h>
#include <sys/types.h>
#include <assert.h>

#include "radixtree_bitops.h"



/*!
	@function rtree_bits_lr_compare
	@abstract returns the first bit position where the two strings differ
	@discussion left-to-right comparison for prefixes
	@param a
	@param b
	@param comp_len length to compare in bits, must be <= the smallest of the two strings (no checking is done)
	@result the first bit position where the two stirngs diff. zero means
		the first bits differed
*/
rt_bslen_t rtree_bits_lr_compare( rt_bit_string_t *a, rt_bit_string_t *b, rt_bslen_t comp_len )
{
	rt_bslen_t byte_comp_len = rt_bit_byte( comp_len ) + ((comp_len&0x7) ==0?0:1);
	rt_bslen_t d, i, j;
	int m;
	for( i = 0; i < byte_comp_len; i ++ )
	{
		rt_bstype_t x = a->d[i] ^ b->d[i];
		if( x )
		{
			d = 0;
			m = 0x80;
			j = rt_bits_sz(i);
			while( !(m & x) && j+d < comp_len )
				d ++, m >>= 1;
			return j + d;
		}
	}
	return comp_len;
}

rt_bit_string_t *rtree_bits( rt_bslen_t bytes_len )
{
	rt_bit_string_t *r = (rt_bit_string_t *)malloc( sizeof(rt_bit_string_t)+bytes_len );
	r->len = rt_bits_sz( bytes_len );	/* length in bits */
	return r;
}
rt_bit_string_t *rtree_bits_from_bytes( rt_bstype_t *d, rt_bslen_t bytes_len )
{
	rt_bit_string_t *r = rtree_bits( bytes_len );
	memcpy( r->d, d, bytes_len );
	return r;
}
rt_bit_string_t *rtree_bits_dup( rt_bit_string_t *bstr )
{
	rt_bslen_t bytes_len = rt_bstr_sz( bstr );
	rt_bit_string_t *r = rtree_bits( bytes_len );
	memcpy( r->d, bstr->d, bytes_len );
	r->len = bstr->len;
	return r;
}

rt_bit_string_t *rtree_bits_split( rt_bit_string_t *a, rt_bslen_t bit_pos )
{
	/* returns a new bitstring from the right side of the split position bit_pos */
	rt_bit_string_t *r = NULL;
	rt_bslen_t byte_pos = rt_bit_byte( bit_pos );
	rt_bslen_t r_bytes_len, i;
	rt_bslen_t r_desired_len = rt_bit_byte(a->len) - byte_pos;
	int r_extra_byte = ((bit_pos&0x7)==0?0:1);
	unsigned char minor_bit_pos;
	
	assert( bit_pos < a->len );
	r_desired_len = (r_desired_len<=0?1:r_desired_len)+r_extra_byte;
	r = rtree_bits_from_bytes( &a->d[byte_pos], r_desired_len );
	if( (bit_pos&0x7) == 0 )
		return r;
	r->len = a->len - bit_pos;	
	r_bytes_len = rt_bstr_sz(r) + r_extra_byte;
	assert(r_bytes_len);
	
	minor_bit_pos = (bit_pos&0x7);

//	printf("r_bytes_len: %d\n", (int)r_bytes_len);
//	printf("byte_pos: %d\n", (int)byte_pos);
//	printf("r->len: %d\n", (int)r->len);	
//	printf("minor_bit_pos: %d\n", (int)minor_bit_pos);

	for( i = 0; i < r_desired_len; i ++)
	{
		r->d[i] <<= minor_bit_pos;
		if( (i+1) < r_bytes_len )
			r->d[i] |= (r->d[i+1] >> (8-minor_bit_pos));
	}
//	r->len -= minor_bit_pos;
	return r;
}

rt_bit_string_t *rtree_bits_concat( rt_bit_string_t *a, rt_bit_string_t *b )
{
	if( a == NULL )
		return rtree_bits_dup(b);
	/* returns a new bitstring: a + b */
	rt_bslen_t a_total_bytes = rt_bstr_sz(a);	
	rt_bslen_t b_total_bytes = rt_bstr_sz(b);		
	rt_bslen_t minor_bit_pos = (a->len&0x7);
	rt_bslen_t new_bits_len = a->len+b->len;
	rt_bslen_t i, j;
	rt_bit_string_t *r = rtree_bits( a_total_bytes+b_total_bytes );	/* may allocate 1 extra byte */
	r->len = new_bits_len;
	
//	printf("\tr->len: %d\n", r->len);
//	printf("\trt_bit_byte( a->len ): %d\n",  rt_bit_byte( a->len ) );
	memcpy( r->d, a->d, rt_bit_byte( a->len ) );

	r->d[rt_bit_byte( a->len )] = a->d[rt_bit_byte( a->len )] & ((0xFF>>(8-minor_bit_pos))<<(8-minor_bit_pos));
	
	/* copy b starting at [a->len] */

//	if(minor_bit_pos == 0)
//		minor_bit_pos = 8;
//	printf("\tminor_bit_pos: %d\n", minor_bit_pos );		
	for( i = 0, j = rt_bit_byte(a->len); i < b_total_bytes; i ++, j ++ )
	{
//		r->d[j] |= (b->d[i]>>(8-minor_bit_pos));
//		r->d[j+1] = (b->d[i]<<minor_bit_pos);
		r->d[j] |= (b->d[i]>>minor_bit_pos);
		r->d[j+1] = (b->d[i]<<(8-minor_bit_pos));
	}
	return r;
}
void rtree_bits_dump( rt_bit_string_t *a, int bShowLocs )
{
	rt_bslen_t comp_len = a->len;
//	rt_bslen_t byte_comp_len = rt_bit_byte( comp_len ) + (comp_len - (comp_len&~0x7) ==0?0:1);
	rt_bslen_t byte_comp_len = rt_bstr_sz(a);

//	printf("bit len: %d, byte len: %d, rt_bit_byte: %d\n", (int)comp_len, (int)byte_comp_len, rt_bit_byte(comp_len));
	int i, b, bc;

	for( i = 0, bc = 0; i < byte_comp_len; i ++ )
	{
		b = 8;
		do
		{
			b--;
			if( bc < comp_len )
				printf("%d", (rt_bit_mask(b) & a->d[i] ? 1 : 0) );
			else
				printf("x");
			bc ++;
		} while(b);
		printf(" ");
	}
			
	printf("\n");
	if( bShowLocs )
	{
		for( i = 0, bc = 0; i < byte_comp_len; i ++ )
		{
			for( b = 0; b < 8; b ++ )
			{
				if( bc < comp_len )
					printf("%.1d",1+(bc%8) );
				else
					printf("x");
				bc ++;
			
			}
			printf(" ");
		}
		printf("\n");
	}
}
#ifdef RTREE_BITOPS_MAIN



int main( int argc, char *argv[] )
{
	int i, x;
	unsigned char d1[] = {
		0xB3,
		0x0B,
		0x60 /*21 bits */
	};
	unsigned char d2[] = {
		//10110011 00001011 01100011 10101011 00101001 10001xxx		
		0xB3,
		0x0B,
		0x63,
		0xAB,
		0x29,
		0x88
	};
	rt_bit_string_t *a = rtree_bits_from_bytes( "12345678", 8 );
	rt_bit_string_t *b = rtree_bits_from_bytes( "13345678", 8 );
	rt_bit_string_t *c = rtree_bits_from_bytes( "ab", 2 );
	rt_bit_string_t *d = rtree_bits_from_bytes( "cd", 2 );
	rt_bit_string_t *e = rtree_bits_from_bytes( "ab", 2 );	
	rt_bit_string_t *t1 = rtree_bits_from_bytes( d2, 6 );
	t1->len = 45;	
//	t1->len = 21;
	
	rt_bit_string_t *r;
#ifdef blah
//	rtree_bits_dump(a);
	/*
	for( i = 0; i < 10; i ++ )
	{
		b->len = b->len-1;
		rtree_bits_dump(b);
		printf("----\n");
		rt_bslen_t diff_pos = rtree_bits_lr_compare( a, b, b->d );
		for( x = 0; x < diff_pos; x ++)
			printf(" ");
		printf("^\n");
		printf("diff_pos: %d\n", (int)diff_pos);

	}
	*/

	rtree_bits_dump(c, 1);
	rtree_bits_dump(d, 1);
	r = rtree_bits_concat(rtree_bits_from_bytes( "123", 3 ), rtree_bits_from_bytes( "456", 3) );	
	for( i = 0; i < r->len; i ++ )
		printf("%c", r->d[i]);
	printf("\n");
//	r = rtree_bits_concat(rtree_bits_split(c,12), d);
//	r = rtree_bits_split(c,12);
//	c->len = 12;
	r = rtree_bits_concat(c, r);
	rtree_bits_dump(r, 0);
	printf("r->len: %d", r->len);
#endif

	r = rtree_bits_split( t1, 21 );
	rtree_bits_dump(t1, 0);
	rtree_bits_dump(r, 0);
	printf("r->len: %d\n", r->len);

	for( i = 0; i < 16; i ++ )
	{
		r = rtree_bits_split(c, i);
//		printf("split: "); rtree_bits_dump(r, 0);
		e->len = i;
		r = rtree_bits_concat((i==0?NULL:e), r);
		printf("joind: "); rtree_bits_dump(r, 0);
		
	}
//	r = rtree_bits_split(a, 16);
//	rtree_bits_dump(r);
	return EXIT_SUCCESS;
}


#endif